/////////////////////////////////////////////////////////////////////////
//
// Amuse Engine SDK - core/math/matrix
// Copyright (c) 2014.  All Rights Reserved
//
// File:		AEMatrix3f.h
// Author:		Gianluca Belardelli
// Date:		31/07/2013
//
// Revision:	30/01/2014 - Cambiata l'interfaccia e il nome della 
//				classe per supportare i vettori AEVector4f.
//
/////////////////////////////////////////////////////////////////////////
#ifndef _AEMATRIX3F_INL_
#define _AEMATRIX3F_INL_

AE_FORCEINLINE const AEMatrix3f& AE_CALL AEMatrix3f::GetIdentity( void )
{
	union
	{
		const AEQuadFloat32	*lpqfValue;
		const AEMatrix3f 	*lpMatrix;
	}quad2Matrix;
	
	quad2Matrix.lpqfValue = g_vectorfConstants + AE_QUADREAL_1000;
	return *quad2Matrix.lpMatrix;
}

AE_FORCEINLINE AEMatrix3f::AEMatrix3f( AEFLOAT32 c0r0, AEFLOAT32 c1r0, AEFLOAT32 c2r0,
                                        AEFLOAT32 c0r1, AEFLOAT32 c1r1, AEFLOAT32 c2r1,
                                        AEFLOAT32 c0r2, AEFLOAT32 c1r2, AEFLOAT32 c2r2,
                                        AEMatrixLayout aemlLayout )
{
	Set( c0r0, c1r0, c2r0,
		 c0r1, c1r1, c2r1,
		 c0r2, c1r2, c2r2,
		 aemlLayout );
}

AE_FORCEINLINE AEMatrix3f::AEMatrix3f( const AEMatrix3f &mtCopy )
{
	m_vcColumn0 = mtCopy.GetColumn<0>();
	m_vcColumn1 = mtCopy.GetColumn<1>();
	m_vcColumn2 = mtCopy.GetColumn<2>();
}

AE_FORCEINLINE void AEMatrix3f::SetIdentity( void )
{
	m_vcColumn0 = AEVector4f::GetConstant<AE_QUADREAL_1000>();
	m_vcColumn1 = AEVector4f::GetConstant<AE_QUADREAL_0100>();
	m_vcColumn2 = AEVector4f::GetConstant<AE_QUADREAL_0010>();
}

AE_FORCEINLINE void AEMatrix3f::SetZero( void )
{
	m_vcColumn0.SetZero();
	m_vcColumn1.SetZero();
	m_vcColumn2.SetZero();
}

AE_FORCEINLINE void AEMatrix3f::Set( AEFLOAT32 c0r0, AEFLOAT32 c1r0, AEFLOAT32 c2r0,
                                      AEFLOAT32 c0r1, AEFLOAT32 c1r1, AEFLOAT32 c2r1,
                                      AEFLOAT32 c0r2, AEFLOAT32 c1r2, AEFLOAT32 c2r2,
                                      AEMatrixLayout aemlLayout )
{
	if( aemlLayout == AEML_ColumnMajor)
	{
		m_vcColumn0.Set( c0r0, c0r1, c0r2, 0.0f );
		m_vcColumn1.Set( c1r0, c1r1, c1r2, 0.0f );
		m_vcColumn2.Set( c2r0, c2r1, c2r2, 0.0f );
	}
	else
	{
		m_vcColumn0.Set( c0r0, c1r0, c2r0, 0.0f );
		m_vcColumn1.Set( c0r1, c1r1, c2r1, 0.0f );
		m_vcColumn2.Set( c0r2, c1r2, c2r2, 0.0f );
	}
}

AE_FORCEINLINE void AEMatrix3f::Set( const AEFLOAT32 *lpCopyFrom, AEMatrixLayout aemlLayout )
{
	AEASSERT( lpCopyFrom != NULL );
	
	if( aemlLayout == AEML_ColumnMajor)
	{
		m_vcColumn0.Set( lpCopyFrom[0], lpCopyFrom[1], lpCopyFrom[2], 0.0f );
		m_vcColumn1.Set( lpCopyFrom[3], lpCopyFrom[4], lpCopyFrom[5], 0.0f );
		m_vcColumn2.Set( lpCopyFrom[6], lpCopyFrom[7], lpCopyFrom[8], 0.0f );
		
	}
	else
	{
		m_vcColumn0.Set( lpCopyFrom[0], lpCopyFrom[3], lpCopyFrom[6], 0.0f );
		m_vcColumn1.Set( lpCopyFrom[1], lpCopyFrom[4], lpCopyFrom[7], 0.0f );
		m_vcColumn2.Set( lpCopyFrom[2], lpCopyFrom[5], lpCopyFrom[8], 0.0f );
	}
}

AE_FORCEINLINE void AEMatrix3f::Get( AEFLOAT32 *lpCopyTo, AEMatrixLayout aemlLayout ) const
{
	AEASSERT( lpCopyTo != NULL );
	
	if( aemlLayout == AEML_ColumnMajor )
	{
		lpCopyTo[0] = m_vcColumn0.GetComponent<0>();
		lpCopyTo[1] = m_vcColumn0.GetComponent<1>();
		lpCopyTo[2] = m_vcColumn0.GetComponent<2>();
		
		lpCopyTo[3] = m_vcColumn1.GetComponent<0>();
		lpCopyTo[4] = m_vcColumn1.GetComponent<1>();
		lpCopyTo[5] = m_vcColumn1.GetComponent<2>();
		
		lpCopyTo[6] = m_vcColumn2.GetComponent<0>();
		lpCopyTo[7] = m_vcColumn2.GetComponent<1>();
		lpCopyTo[8] = m_vcColumn2.GetComponent<2>();
	}
	else
	{
		lpCopyTo[0] = m_vcColumn0.GetComponent<0>();
		lpCopyTo[1] = m_vcColumn1.GetComponent<0>();
		lpCopyTo[2] = m_vcColumn2.GetComponent<0>();
		
		lpCopyTo[3] = m_vcColumn0.GetComponent<1>();
		lpCopyTo[4] = m_vcColumn1.GetComponent<1>();
		lpCopyTo[5] = m_vcColumn2.GetComponent<1>();
		
		lpCopyTo[6] = m_vcColumn0.GetComponent<2>();
		lpCopyTo[7] = m_vcColumn1.GetComponent<2>();
		lpCopyTo[8] = m_vcColumn2.GetComponent<2>();
	}
}

template <AEINT32 N> 
AE_FORCEINLINE void AEMatrix3f::SetColumn( AEVector4fRefParam vcValues )
{
	AEASSERT( N <= 2 );
	(&m_vcColumn0)[N] = vcValues;
}

AE_FORCEINLINE AEVector4f &AEMatrix3f::GetColumn( AEINT32 nIdx )
{
	AEASSERT( nIdx <= 2 );
	return (&m_vcColumn0)[nIdx];
}

AE_FORCEINLINE const AEVector4f &AEMatrix3f::GetColumn( AEINT32 nIdx ) const
{
	AEASSERT( nIdx <= 2 );
	return (&m_vcColumn0)[nIdx];
}

template <AEINT32 N>
AE_FORCEINLINE const AEVector4f &AEMatrix3f::GetColumn( void ) const
{
	return (&m_vcColumn0)[N];
}

template <AEINT32 N>
AE_FORCEINLINE AEVector4f &AEMatrix3f::GetColumn( void )
{
	return (&m_vcColumn0)[N];
}

template <AEINT32 ROW, AEINT32 COL>	
AE_FORCEINLINE const AESimdFloat32 AEMatrix3f::GetElement( void ) const
{
	return (&m_vcColumn0)[COL].GetComponent<ROW>();
}

template <AEINT32 ROW, AEINT32 COL> 
AE_FORCEINLINE void AEMatrix3f::SetElement( AESimdFloat32ConstRef fValue )
{
	(&m_vcColumn0)[COL].SetComponent<ROW>( fValue );
}

AE_FORCEINLINE void AEMatrix3f::SetRows( AEVector4fRefParam vcRow0, AEVector4fRefParam vcRow1, AEVector4fRefParam vcRow2 )
{
#if (AE_CONFIG_SIMD == AE_CONFIG_SIMD_ENABLED)
	m_vcColumn0 = vcRow0;
	m_vcColumn1 = vcRow1;
	m_vcColumn2 = vcRow2;
	AE_VECTORTRANSPOSE3F( m_vcColumn0, m_vcColumn1, m_vcColumn2 );
#else
	AEVector4f c0,c1,c2,c3;

	c0.Set( vcRow0(0), vcRow1(0), vcRow2(0), 0.0f );
	c1.Set( vcRow0(1), vcRow1(1), vcRow2(1), 0.0f );
	c2.Set( vcRow0(2), vcRow1(2), vcRow2(2), 0.0f );
	c3.Set( vcRow0(3), vcRow1(3), vcRow2(3), 0.0f );
	
	SetColumn<0>(c0);
	SetColumn<1>(c1);
	SetColumn<2>(c2);
#endif
}

AE_FORCEINLINE void AEMatrix3f::GetRows( AEVector4f &vcRow0, AEVector4f &vcRow1, AEVector4f &vcRow2 ) const
{
#if (AE_CONFIG_SIMD == AE_CONFIG_SIMD_ENABLED)
	AEVector4f c0 = GetColumn<0>();
	AEVector4f c1 = GetColumn<1>();
	AEVector4f c2 = GetColumn<2>();

	AE_VECTORTRANSPOSE3F( c0, c1, c2 );
#else
	AEVector4f c0, c1, c2;

	c0.Set( m_vcColumn0(0), m_vcColumn1(0), m_vcColumn2(0), 0.0f );
	c1.Set( m_vcColumn0(1), m_vcColumn1(1), m_vcColumn2(1), 0.0f );
	c2.Set( m_vcColumn0(2), m_vcColumn1(2), m_vcColumn2(2), 0.0f );
#endif

	vcRow0 = c0;
	vcRow1 = c1;
	vcRow2 = c2;
}

AE_FORCEINLINE void AEMatrix3f::SetCols( AEVector4fRefParam vcCol0, AEVector4fRefParam vcCol1, AEVector4fRefParam vcCol2 )
{
	m_vcColumn0 = vcCol0;
	m_vcColumn1 = vcCol1;
	m_vcColumn2 = vcCol2;
}

AE_FORCEINLINE void AEMatrix3f::GetCols( AEVector4f &vcCol0, AEVector4f &vcCol1, AEVector4f &vcCol2 ) const
{
	vcCol0 = m_vcColumn0;
	vcCol1 = m_vcColumn1;
	vcCol2 = m_vcColumn2;
}

AE_FORCEINLINE void AEMatrix3f::GetRow( AEINT32 nRow, AEVector4f &vcOut ) const
{
	vcOut.Set( m_vcColumn0( nRow ), m_vcColumn1( nRow ), m_vcColumn2( nRow ), 0.0f );
}

template <AEINT32 I>
AE_FORCEINLINE void AEMatrix3f::GetRow( AEVector4f &vcOut ) const
{
	vcOut.Set( GetElement<I,0>(), GetElement<I,1>(), GetElement<I,2>(), 0.0f );
}

AE_FORCEINLINE void AEMatrix3f::AddDiagonal( AESimdFloat32ConstRef fDiagonal )
{
#if (AE_CONFIG_SIMD == AE_CONFIG_SIMD_ENABLED)
	m_vcColumn0.AddMul( fDiagonal, AEVector4f::GetConstant<AE_QUADREAL_1000>() );
	m_vcColumn1.AddMul( fDiagonal, AEVector4f::GetConstant<AE_QUADREAL_0100>() );
	m_vcColumn2.AddMul( fDiagonal, AEVector4f::GetConstant<AE_QUADREAL_0010>() );
#else
	const AEFLOAT32 fAdd = fDiagonal.GetReal();
	m_vcColumn0(0) += fAdd;
	m_vcColumn1(1) += fAdd;
	m_vcColumn2(2) += fAdd;
#endif
}

AE_FORCEINLINE void AEMatrix3f::AddMul( AESimdFloat32ConstRef fScale, const AEMatrix3f &mtMatrix )
{
	m_vcColumn0.AddMul( fScale, mtMatrix.GetColumn<0>() );
	m_vcColumn1.AddMul( fScale, mtMatrix.GetColumn<1>() );
	m_vcColumn2.AddMul( fScale, mtMatrix.GetColumn<2>() );
}

AE_FORCEINLINE void AEMatrix3f::Add( const AEMatrix3f &mtMatrix )
{
	m_vcColumn0.Add( mtMatrix.GetColumn<0>() );
	m_vcColumn1.Add( mtMatrix.GetColumn<1>() );
	m_vcColumn2.Add( mtMatrix.GetColumn<2>() );
}

AE_FORCEINLINE void AEMatrix3f::Sub( const AEMatrix3f &mtMatrix )
{
	m_vcColumn0.Sub( mtMatrix.GetColumn<0>() );
	m_vcColumn1.Sub( mtMatrix.GetColumn<1>() );
	m_vcColumn2.Sub( mtMatrix.GetColumn<2>() );
}

AE_FORCEINLINE void AEMatrix3f::Mul( AESimdFloat32ConstRef fScale )
{
	m_vcColumn0.Mul( fScale );
	m_vcColumn1.Mul( fScale );
	m_vcColumn2.Mul( fScale );
}

AE_FORCEINLINE void AEMatrix3f::MultiplyVector( AEVector4fRefParam vcVectorIn, AEVector4f &vcResultOut ) const
{
	const AESimdFloat32 xb = vcVectorIn.GetComponent<0>();
	const AESimdFloat32 yb = vcVectorIn.GetComponent<1>();
	const AESimdFloat32 zb = vcVectorIn.GetComponent<2>();

	AEVector4f t0;
	t0.SetMul( xb, m_vcColumn0 );
	t0.AddMul( yb, m_vcColumn1 );
	t0.AddMul( zb, m_vcColumn2 );

	vcResultOut = t0;
}

AE_FORCEINLINE void AEMatrix3f::operator= ( const AEMatrix3f &mtSource )
{
	m_vcColumn0 = mtSource.GetColumn<0>();
	m_vcColumn1 = mtSource.GetColumn<1>();
	m_vcColumn2 = mtSource.GetColumn<2>();
}

AE_FORCEINLINE AEFLOAT32 &AEMatrix3f::operator() ( AEINT32 nRow, AEINT32 nCol )
{
	return GetColumn(nCol)(nRow);
}

AE_FORCEINLINE const AEFLOAT32 &AEMatrix3f::operator() ( AEINT32 nRow, AEINT32 nCol ) const
{
	return GetColumn(nCol)(nRow);
}

AE_FORCEINLINE void AEMatrix3f::operator+= ( const AEMatrix3f &matRhs )
{
	Add( matRhs );
}

AE_FORCEINLINE void AEMatrix3f::operator-= ( const AEMatrix3f &matRhs )
{
	Sub( matRhs );
}

AE_FORCEINLINE void AEMatrix3f::operator*= ( AESimdFloat32ConstRef fValue )
{
	Mul( fValue );
}

AE_FORCEINLINE void AEMatrix3f::operator/= ( AESimdFloat32ConstRef fValue )
{
	const AEFLOAT32 fTemp = 1.0f / fValue;
	Mul( fTemp );
}

AE_FORCEINLINE AEBOOL32 AEMatrix3f::IsOk( void ) const
{
	return ( m_vcColumn0.IsOk<3>() && m_vcColumn1.IsOk<3>() && m_vcColumn2.IsOk<3>() );
}

AE_FORCEINLINE void AEMatrix3f::SetTranspose( const AEMatrix3f &mtSource )
{
#if (AE_CONFIG_SIMD == AE_CONFIG_SIMD_ENABLED)
	AEVector4f c0 = mtSource.GetColumn<0>();
	AEVector4f c1 = mtSource.GetColumn<1>();
	AEVector4f c2 = mtSource.GetColumn<2>();
	
	AE_VECTORTRANSPOSE3F(c0,c1,c2);
	
	m_vcColumn0 = c0;
	m_vcColumn1 = c1;
	m_vcColumn2 = c2;
#else
	AEMatrix3f	cs = mtSource;
	m_vcColumn0.Set( cs.GetElement<0,0>(), cs.GetElement<0,1>(), cs.GetElement<0,2>(), cs.GetElement<0,2>() );
	m_vcColumn1.Set( cs.GetElement<1,0>(), cs.GetElement<1,1>(), cs.GetElement<1,2>(), cs.GetElement<1,2>() );
	m_vcColumn2.Set( cs.GetElement<2,0>(), cs.GetElement<2,1>(), cs.GetElement<2,2>(), cs.GetElement<2,2>() );
#endif
}

AE_FORCEINLINE void AEMatrix3f::SetMul( const AEMatrix3f &mMatrix1, const AEMatrix3f &mMatrix2 )
{
	AEASSERT( this != &mMatrix1 );

	m_vcColumn0.SetRotatedDir( mMatrix1, mMatrix2.GetColumn<0>() );
	m_vcColumn1.SetRotatedDir( mMatrix1, mMatrix2.GetColumn<1>() );
	m_vcColumn2.SetRotatedDir( mMatrix1, mMatrix2.GetColumn<2>() );
}

AE_FORCEINLINE void AEMatrix3f::SetMul( AESimdFloat32ConstRef fScale, const AEMatrix3f &mtMatrix )
{
	m_vcColumn0.SetMul( fScale, mtMatrix.GetColumn<0>() );
	m_vcColumn1.SetMul( fScale, mtMatrix.GetColumn<1>() );
	m_vcColumn2.SetMul( fScale, mtMatrix.GetColumn<2>() );
}

AE_FORCEINLINE void AEMatrix3f::SetDiagonalMul( AEVector4fRefParam vcDiag, const AEMatrix3f &mtMatrix )
{
	m_vcColumn0.SetMul( vcDiag, mtMatrix.GetColumn<0>() );
	m_vcColumn1.SetMul( vcDiag, mtMatrix.GetColumn<1>() );
	m_vcColumn2.SetMul( vcDiag, mtMatrix.GetColumn<2>() );
}

AE_FORCEINLINE void AEMatrix3f::SetSubDiagonal( const AEMatrix3f &mtMatrix , AEVector4fRefParam vcDiagVector )
{
	m_vcColumn0.SetSubMul( mtMatrix.GetColumn<0>(), AEVector4f::GetConstant<AE_QUADREAL_1000>(), vcDiagVector.GetComponent<0>() );
	m_vcColumn1.SetSubMul( mtMatrix.GetColumn<1>(), AEVector4f::GetConstant<AE_QUADREAL_0100>(), vcDiagVector.GetComponent<1>() );
	m_vcColumn2.SetSubMul( mtMatrix.GetColumn<2>(), AEVector4f::GetConstant<AE_QUADREAL_0010>(), vcDiagVector.GetComponent<2>() );
}

AE_FORCEINLINE void AEMatrix3f::SetSubDiagonal( const AEMatrix3f &mtMatrix , AESimdFloat32ConstRef fDiagonal )
{
	m_vcColumn0.SetSubMul( mtMatrix.GetColumn<0>(), AEVector4f::GetConstant<AE_QUADREAL_1000>(), fDiagonal );
	m_vcColumn1.SetSubMul( mtMatrix.GetColumn<1>(), AEVector4f::GetConstant<AE_QUADREAL_0100>(), fDiagonal );
	m_vcColumn2.SetSubMul( mtMatrix.GetColumn<2>(), AEVector4f::GetConstant<AE_QUADREAL_0010>(), fDiagonal );
}

#endif
// _AEMATRIX3_INL_
